#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "HC_SR04.h"
#include "MPU6050.h"
#include "Motor.h"
#include "Encoder.h"
#include <math.h>
#include "Exti.h"
#define EPSILON 0.0001

int16_t AX, AY, AZ, GX, GY, GZ;			//定义用于存放各个数据的变量
uint8_t KeyNum;	                	  //定义用于接收按键键码的变量

uint8_t ID;								//定义用于存放ID号的变量
uint8_t cnt;
//float Target=40;
//float	Actual, Out;
//float Kp=0.2;
//float Ki, Kd;
//float Error0, Error1, ErrorInt;
float length=0;

int Encoder_motor1=0;
int Encoder_motor2=0;


int main(void)
{
	/*模块初始化*/
	OLED_Init();       //oled初始化
	HC_SR04Config();   //超声波初始化
	MPU6050_Init();	   //姿态初始化
	Motor_Init();      //电机初始化
	Encoder_Init_TIM2();
	Encoder_Init_TIM4();
	MPU6050_EXTI_Init();
	//Encoder_Get2();
  OLED_ShowString(2,1,"length:");
	NVIC_Config();
	while (1)
	{
		//MPU6050_GetData(&AX, &AY, &AZ, &GX, &GY, &GZ);		//获取MPU6050的数据
		length=Getlength();
		Encoder_motor1=Read_Encoder(2);
		Encoder_motor2=Read_Encoder(4);
		OLED_ShowNum(2,8,length,3);
		OLED_ShowSignedNum(3, 1, GX, 5);
		OLED_ShowSignedNum(3, 8, GY, 5);
		OLED_ShowSignedNum(4, 1, GZ, 5);
//		OLED_ShowSignedNum(3, 1, Encoder_motor1, 5);
//		OLED_ShowSignedNum(4, 1, Encoder_motor2, 5);
		OLED_ShowSignedNum(1,1,cnt,5);
		
		
		
	}
}
/**
  * 函    数：EXTI9_5外部中断函数
  * 参    数：无
  * 返 回 值：无
  * 注意事项：此函数为中断函数，无需调用，中断触发后自动执行
  *           函数名为预留的指定名称，可以从启动文件复制
  *           请确保函数名正确，不能有任何差异，否则中断函数将不能进入
  */
void EXTI9_5_IRQHandler(void)
{
	if (EXTI_GetITStatus(EXTI_Line5) == SET)
  {
		if (GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_5) == 0)
		{
			EXTI_ClearITPendingBit(EXTI_Line5);
			MPU6050_GetData(&AX, &AY, &AZ, &GX, &GY, &GZ);		//获取MPU6050的数据
			//cnt++;			
			Motor_SetSpeed(1,50);
			Motor_SetSpeed(2,50);
		}
		
	}
}



